#!/usr/bin/env python3
import rospy
from std_msgs.msg import Int32, String
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import tf
import serial
import math

# 定义机器人的参数
wheel_radius = 0.05  # 轮子半径（米）
wheel_base = 0.3     # 两轮之间的距离（米）
ticks_per_revolution = 4096  # 每转一圈的编码器脉冲数

# 全局变量
x = 0.0
y = 0.0
th = 0.0

# 串口对象
ser = None

# 回调函数处理左轮编码器数据
def left_encoder_callback(msg):
    global x, y, th, last_left_ticks
    current_left_ticks = msg.data
    delta_left_ticks = current_left_ticks - last_left_ticks
    last_left_ticks = current_left_ticks

    delta_left_distance = (2 * math.pi * wheel_radius * delta_left_ticks) / ticks_per_revolution

    # 更新位姿
    update_pose(delta_left_distance, 0.0)

# 回调函数处理右轮编码器数据
def right_encoder_callback(msg):
    global x, y, th, last_right_ticks
    current_right_ticks = msg.data
    delta_right_ticks = current_right_ticks - last_right_ticks
    last_right_ticks = current_right_ticks

    delta_right_distance = (2 * math.pi * wheel_radius * delta_right_ticks) / ticks_per_revolution

    # 更新位姿
    update_pose(0.0, delta_right_distance)

# 处理cmd_vel消息
def cmd_vel_callback(msg):
    global ser
    linear = msg.linear.x
    angular = msg.angular.z

    # 将速度转换为电机控制信号
    left_speed = (linear - (angular * wheel_base / 2.0)) / wheel_radius
    right_speed = (linear + (angular * wheel_base / 2.0)) / wheel_radius

    # 构造串口命令
    command = "L{}R{}\n".format(int(left_speed), int(right_speed))

    if ser and ser.is_open:
        # 通过串口发送命令
        ser.write(command.encode())


# 更新位姿
def update_pose(delta_left, delta_right):
    global x, y, th, last_time, odom_pub

    # 计算线速度和角速度
    linear_velocity = (delta_left + delta_right) / 2.0
    angular_velocity = (delta_right - delta_left) / wheel_base

    # 计算时间间隔
    current_time = rospy.Time.now()
    dt = (current_time - last_time).to_sec()
    last_time = current_time

    # 更新位姿
    delta_x = (linear_velocity * math.cos(th)) * dt
    delta_y = (linear_velocity * math.sin(th)) * dt
    delta_th = angular_velocity * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # 发布里程计信息
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # 设置位置
    odom.pose.pose.position.x = x
    odom.pose.pose.position.y = y
    odom.pose.pose.position.z = 0.0
    odom.pose.pose.orientation = tf.transformations.quaternion_from_euler(0, 0, th)

    # 设置速度
    odom.child_frame_id = "base_footprint"
    odom.twist.twist.linear.x = linear_velocity
    odom.twist.twist.angular.z = angular_velocity

    # 发布消息
    odom_pub.publish(odom)

    # rospy.loginfo("x: %.2f, y: %.2f", x, y)

def main():
    global last_left_ticks, last_right_ticks, last_time, odom_pub, ser

    # 初始化ROS节点
    rospy.init_node('base_controller_node')

    # 打开串口
    # try:
    #     ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
    #     rospy.loginfo("Serial Port initialized")
    # except serial.SerialException as e:
    #     rospy.logerr("Unable to open port: %s", str(e))
    #     return

    # 订阅编码器数据
    last_left_ticks = 0
    last_right_ticks = 0
    rospy.Subscriber('left_encoder', Int32, left_encoder_callback)
    rospy.Subscriber('right_encoder', Int32, right_encoder_callback)

    # 订阅cmd_vel话题
    rospy.Subscriber('cmd_vel', Twist, cmd_vel_callback)

    # 创建里程计发布者
    odom_pub = rospy.Publisher('encode_odom', Odometry, queue_size=10)

    # 初始化时间
    last_time = rospy.Time.now()

    # 进入ROS主循环
    rospy.spin()

if __name__ == '__main__':
    main()
